﻿#!/usr/bin/python3
#coding=utf-8
import cv2
import sys
import numpy as np
import time
import math
import threading
#import Serial_Servo_Running as SSR
#import PWMServo

#stream = "http://127.0.0.1:8080/?action=stream?dummy=param.mjpg"
#cap = cv2.VideoCapture(stream)
cap = cv2.VideoCapture("video/gate2.mp4")
orgFrame = None
ret = False

c = 80
resize_width = 4*c
resize_height = 3*c
ori_width = cap.get(3)
ori_height = cap.get(4)

go_straight = '1'
turn_left = 'turn_left'
turn_right = 'turn_right'

get_gate = False
promit = False

#PWMServo.setServo(1, 500, 500)
#PWMServo.setServo(2, 1500, 500)
#time.sleep(0.5)


def get_image():
    global orgFrame
    global ret
    global cap
    while True:
        if cap.isOpened():
            ret, orgFrame = cap.read()
            #orgFrame = cv2.flip(orgFrame,0)
            time.sleep(0.1)
        else:
            time.sleep(0.01)

th1 = threading.Thread(target = get_image)
th1.setDaemon(True)
th1.start()


def move():
    global get_gate, promit
    global state
    while True:
        if get_gate is False and promit:
            time.sleep(2)
            state = "go"
            #get_gate = False
            #promit = False
            #SSR.running_action_group(go_straight, 6)
        else:
            state = "stop"
            time.sleep(0.01)
th2 = threading.Thread(target=move)
th2.setDaemon(True)
th2.start() 


def detection(orgImage):
    global get_gate
    global promit
    orgImg = cv2.resize(orgImage,(resize_width,resize_height))
    orgImg = cv2.cvtColor(orgImg, cv2.COLOR_BGR2GRAY)
    orgImg = cv2.GaussianBlur(orgImg,(3,3),0)
    _, Imask = cv2.threshold(orgImg,50,255,cv2.THRESH_BINARY_INV)
    Imask = cv2.morphologyEx(Imask, cv2.MORPH_OPEN, np.ones((3,3),np.uint8))
    cv2.imshow("Imask",Imask)
    _, cnts, _ = cv2.findContours(Imask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_TC89_L1)
    cnt_large  = getAreaMaxContour(cnts, area = 1200)
    if cnt_large is not None:
        rect = cv2.minAreaRect(cnt_large)
        box = np.int0(cv2.boxPoints(rect))
        cv2.drawContours(orgImg, [box], -1,(0,0,255,255), 2)
        get_gate = True
    else:
        if get_gate:
            get_gate = False
            promit = True
            time.sleep(1)
    return orgImg

def getAreaMaxContour(contours,area=1):
    contour_area_max = 0
    area_max_contour = None

    for c in contours :
        contour_area_temp = math.fabs(cv2.contourArea(c))#计算面积并返回绝对值
        if contour_area_temp > contour_area_max : 
            contour_area_max = contour_area_temp
            if contour_area_temp > area:#面积大于1
                area_max_contour = c
    return area_max_contour


while True:
    if orgFrame is not None and ret:
        t1 = cv2.getTickCount()
        outFrame = detection(orgFrame)
        t2 = cv2.getTickCount()
        time_r = (t2-t1)/cv2.getTickFrequency()
        fps = 1.0/time_r
        cv2.putText(outFrame, "state :" + str(state) ,
        (10, outFrame.shape[0] - 35), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 255), 2)
        cv2.putText(outFrame, "fps:" + str(int(fps)),
        (10, outFrame.shape[0] - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 255), 2)#(0, 0, 255)BGR

        cv2.imshow("orgFrame",orgFrame)
        cv2.imshow("outFrame",outFrame)
        cv2.waitKey(1)
    else:
        cv2.destroyAllWindows()
        time.sleep(0.01)
cv2.destroyAllWindows()